#include <stdio.h>
#include <list>
#include <vector>

#define INF 0x7fffffff

struct Edge {
  Edge(int v, int dist) : v(v), dist(dist) { }
  Edge() : v(-1), dist(INF) { }

  int v;
  int dist;
};

typedef std::list<Edge> EdgeList;

class Graph {
public:
  Graph(int n_vertices) : vertices(n_vertices) { }
  void AddEdge(int i, int j, int dist) {
    vertices[i].push_back(Edge(j, dist));
    vertices[j].push_back(Edge(i, dist));
  }
  int n_vertices() const { return vertices.size(); }
  const EdgeList &vertex(int i) const { return vertices[i]; }
private:
  std::vector<EdgeList> vertices;
};

typedef std::list<int> Queue;

Queue::iterator FindNearestVertex(Queue &q, const std::vector<int> &dists) {
  if (q.empty()) return q.end();
  Queue::iterator k = q.begin();
  int dist = dists[*k];
  for (Queue::iterator it = q.begin(); it != q.end(); ++it) {
    if (dists[*it] < dist) {
      k = it;
      dist = dists[*it];
    }
  }
  return k;
}

void Dijkstra(const Graph &g, int i, int j) {
  std::vector<int> dist(g.n_vertices(), INF);
  dist[i] = 0;

  Queue q;
  for (int k = 0; k < dist.size(); ++k) q.push_back(k);

  printf("prepared\n");
  std::vector<int> previous(g.n_vertices(), -1);
  while (!q.empty()) {
    Queue::iterator k = FindNearestVertex(q, dist);

    if (dist[*k] == INF) break;
    if (*k == j) break;

    for (EdgeList::const_iterator it = g.vertex(*k).begin();
         it != g.vertex(*k).end(); ++it) {
      int new_dist = dist[*k] + it->dist;
      if (new_dist < dist[it->v]) {
        dist[it->v] = new_dist;
        previous[it->v] = *k;
      }
    }
    q.erase(k);
  }

  printf("retrieve result\n");
  std::list<int> path;
  int u = j;
  while (previous[u] >= 0) {
    path.push_front(u);
    u = previous[u];
  }
  path.push_front(i);

  for (std::list<int>::const_iterator it = path.begin(); it != path.end(); ++it) {
    printf("%d ", *it);
  }
  printf("\n");
}

int main(int argc, const char *argv[]) {
  Graph g(6);
  g.AddEdge(0, 1, 7);
  g.AddEdge(0, 2, 9);
  g.AddEdge(0, 5, 14);
  g.AddEdge(1, 2, 10);
  g.AddEdge(1, 3, 15);
  g.AddEdge(2, 3, 11);
  g.AddEdge(2, 5, 2);
  g.AddEdge(3, 4, 6);
  g.AddEdge(4, 5, 9);

  Dijkstra(g, 0, 4);
  return 0;
}

